/*
 *  skeletonUtil.c
 *  SAGame
 *
 *  Created by Hiroshi Nomura on 13/01/06.
 *  Copyright 2013 __MyCompanyName__. All rights reserved.
 *
 */

#include <string.h>
#include <stdio.h>
#include "matrixUtil.h"
#include "skeletonUtil.h"

Joint* searchJointByName(Skeleton* skeleton, const char* jointName){
	Joint* jointPtr = skeleton->jointArray;
	for (int i=0; i<skeleton->jointCount; i++) {		
		if (strcmp(jointPtr->jointName, jointName) == 0) {
			return jointPtr;
		}
		jointPtr++;
	}
	return NULL;
}

Joint* searchJointById(Skeleton* skeleton, const short jointId)
{
	Joint* jointPtr = skeleton->jointArray;
	for (int i=0; i<skeleton->jointCount; i++) {			
		if (jointPtr->jointId == jointId) {
			return jointPtr;
		}
		jointPtr++;
	}
	return NULL;
}

void transformLocalPoseToGlobalPose(Skeleton* skeleton, Matrix4x4 mtx)
{
	Joint* jointPtr = skeleton->jointArray;
	for (int i=0; i<skeleton->jointCount; i++) {
		Matrix4x4 localPoseMtx;
		mtxLoadIdentity(localPoseMtx);
		poseToMtx(localPoseMtx, &jointPtr->localPose);
        
		if (jointPtr->parent != NULL) {				
			transformChildPoseToParentPose(jointPtr->parent, localPoseMtx);
		}			
		mtxMultiply(jointPtr->globalPoseMatrix, mtx, localPoseMtx);
		jointPtr++;
		
		/*GLfloat *global = jointPtr->globalPose;				
		NSLog(@"Global Matrix");
		printf("%f, %f, %f, %f\n",global[0],global[1],global[2],global[3]);
		printf("%f, %f, %f, %f\n",global[4],global[5],global[6],global[7]);
		printf("%f, %f, %f, %f\n",global[8],global[9],global[10],global[11]);
		printf("%f, %f, %f, %f\n",global[12],global[13],global[14],global[15]);				
		NSLog(@"\n\n");
		
		GLfloat *inverse = jointPtr->inverseBindPose;
		NSLog(@"Inverse Matrix");
		printf("%f, %f, %f, %f\n",inverse[0],inverse[1],inverse[2],inverse[3]);
		printf("%f, %f, %f, %f\n",inverse[4],inverse[5],inverse[6],inverse[7]);
		printf("%f, %f, %f, %f\n",inverse[8],inverse[9],inverse[10],inverse[11]);
		printf("%f, %f, %f, %f\n",inverse[12],inverse[13],inverse[14],inverse[15]);				
		NSLog(@"\n\n");*/
	}
}

void transformChildPoseToParentPose(Joint* joint, Matrix4x4 mtx)
{
	//Protect Original Matrix
	Matrix4x4 childMtx;
	memcpy(childMtx, mtx, sizeof(Matrix4x4));
	
	Matrix4x4 parentMtx;
	mtxLoadIdentity(parentMtx);
	poseToMtx(parentMtx, &joint->localPose);
	
	mtxMultiply(mtx, parentMtx, childMtx);	
	
	/*printf("Local Matrix\n");
	printf("%f, %f, %f, %f\n",mtx[0],mtx[4],mtx[8],mtx[12]);
	printf("%f, %f, %f, %f\n",mtx[1],mtx[5],mtx[9],mtx[13]);
	printf("%f, %f, %f, %f\n",mtx[2],mtx[6],mtx[10],mtx[14]);
	printf("%f, %f, %f, %f\n",mtx[3],mtx[7],mtx[11],mtx[15]);
	printf("\n\n");*/
	
	if (joint->parent != NULL) {
		transformChildPoseToParentPose(joint->parent, mtx);
	}
}

void poseToMtx(GLfloat *mtx, Pose *pose)
{
	// 3 x 3
	// [ 0 3 6 ]
	// [ 1 4 7 ]
	// [ 2 5 8 ]
	
	// 4 x 4
	// [ 0 4  8 12 ]
	// [ 1 5  9 13 ]
	// [ 2 6 10 14 ]
	// [ 3 7 11 15 ]
	
	GLfloat rotateMtx[9];
	quaternionToMtx(rotateMtx, &pose->quaternion);
    
	mtx[0] = rotateMtx[0]; mtx[1] = rotateMtx[1]; mtx[2]  = rotateMtx[2];
	mtx[4] = rotateMtx[3]; mtx[5] = rotateMtx[4]; mtx[6]  = rotateMtx[5];
	mtx[8] = rotateMtx[6]; mtx[9] = rotateMtx[7]; mtx[10] = rotateMtx[8];
	
	mtx[12] = pose->translate.x;
	mtx[13] = pose->translate.y;
	mtx[14] = pose->translate.z;
	
	mtx[0]  *= pose->scale.x;
	mtx[5]  *= pose->scale.y;
	mtx[10] *= pose->scale.z;
}
